module my_tracking_code
use polymorphic_complextaylor, my_taylor=>real_8, real_8=>real_8
!use my_own_da
implicit none
integer, parameter :: nmag=100,NMULT=4,nvd=3,beta=1,alpha=2,gamma=3,phase=4
private track_mag,track_lattice,track_ONE_TURN
PRIVATE INITIALIZE_MAGNET,INITIALIZE_LATTICE,DESTRUCT_MAGNET,DESTRUCT_LATTICE
integer :: ierror=0
character*255 :: message(10)

!&1
type my_magnet
 type(my_taylor) L,K(1:NMULT)
 integer N,type    ! type=0 drift
  real pos
  character*16 name
 real(dp) twiss(-1:4+NMULT) ! useful information stored here for ploting
 real closed_orbit(2) ! useful information stored here for ploting
end type my_magnet

type my_lattice
 integer n
 type(my_magnet) MAGNET(nmag)
end type my_lattice
!&1

TYPE(MY_LATTICE) LATTICE

  INTERFACE TRACK
     MODULE PROCEDURE track_mag   
     MODULE PROCEDURE track_lattice  
     MODULE PROCEDURE track_ONE_TURN 
  END INTERFACE

  INTERFACE CONSTRUCT
     MODULE PROCEDURE INITIALIZE_MAGNET   
     MODULE PROCEDURE INITIALIZE_LATTICE   
  END INTERFACE

  INTERFACE DESTRUCT
     MODULE PROCEDURE DESTRUCT_MAGNET   
     MODULE PROCEDURE DESTRUCT_LATTICE   
  END INTERFACE

contains

SUBROUTINE INITIALIZE_MAGNET(magnet,N)
IMPLICIT NONE
type(my_magnet), intent(inOUT) :: magnet
INTEGER, INTENT(IN) :: N
INTEGER I

DO I=1,NMULT
call alloc(magnet%K(I))
 magnet%K(I)=0.D0
ENDDO
call alloc(magnet%L)
 magnet%L=0.D0
 magnet%twiss=0.D0
 magnet%closed_orbit=0.D0
 magnet%N=N
 magnet%name='No Name'
 magnet%type=0

END SUBROUTINE INITIALIZE_MAGNET

SUBROUTINE INITIALIZE_LATTICE(LATTICE,N)
IMPLICIT NONE
type(my_lattice), intent(inOUT) :: LATTICE
INTEGER, INTENT(IN) :: N
INTEGER I
 
 LATTICE%N=N
 DO I=1,N
  CALL CONSTRUCT(LATTICE%MAGNET(I),0)
 ENDDO


END SUBROUTINE INITIALIZE_LATTICE

SUBROUTINE DESTRUCT_MAGNET(magnet)
IMPLICIT NONE
type(my_magnet), intent(inOUT) :: magnet
INTEGER I

DO I=1,NMULT
 magnet%K(I)=0.D0
 call kill(magnet%K(I))
ENDDO
 magnet%L=0.D0
 call kill(magnet%L)
 magnet%N=0
 magnet%twiss=0.D0
 magnet%closed_orbit=0.D0
 magnet%type=0

END SUBROUTINE DESTRUCT_MAGNET

SUBROUTINE DESTRUCT_LATTICE(LATTICE)
IMPLICIT NONE
type(my_lattice), intent(inOUT) :: LATTICE
INTEGER I
 
 DO I=1,LATTICE%N
  CALL DESTRUCT(LATTICE%MAGNET(I))
 ENDDO

 LATTICE%N=0

END SUBROUTINE DESTRUCT_LATTICE

!&2
SUBROUTINE track_mag(X,magnet)
IMPLICIT NONE
INTEGER I,j
TYPE(my_taylor) DL,pz2
TYPE(my_taylor), intent(inout) :: X(nvd)
type(my_magnet), intent(in) :: magnet

call alloc(DL,pz2)

DL=magnet%L/magnet%N

if(DL/=0.D0) THEN


 DO I=1,magnet%N
 pz2=(((1.D0+X(3))**2-X(2)**2))
 if(pz2<0) then
  call kill(DL,pz2)
  return
 endif
 X(1)=X(1)+DL*X(2)/SQRT(pz2)
 do j=1,NMULT
  X(2)=X(2)-DL*magnet%K(j)*X(1)**(j-1)
 enddo

 ENDDO
!0.16048137970537104146561520879784
!0.016327074283614542715782165195002
ELSE
if(magnet%type<0) then
  dl=(1.d0 + magnet%K(2))*(cos(magnet%K(1)*twopi)*X(1)+sin(magnet%K(1)*twopi)*X(2)) !*(1.d0 + magnet%K(2)-magnet%K(3)*x(1)**2 ) 
  x(2)=(1.d0 + magnet%K(2))*(cos(magnet%K(1)*twopi)*X(2)-sin(magnet%K(1)*twopi)*X(1)) !*(1.d0 + magnet%K(2)/2.d0 )
  x(1)=dl
  x(1)=x(1)*(1.d0-magnet%K(3)*x(1)**2-magnet%K(4)*x(2)**2) 
else
 do j=1,NMULT
  X(2)=X(2)-magnet%K(j)*X(1)**(j-1)
 enddo
endif
ENDIF

call kill(DL,pz2)

END SUBROUTINE track_mag


SUBROUTINE track_lattice(X,lattice,i1,j1)
IMPLICIT NONE
INTEGER I,j,k1,k2,K,i1,j1
TYPE(my_taylor), intent(inout) :: X(nvd)
TYPE(my_lattice), intent(in) :: lattice

   i=mod(i1,lattice%n); if(i==0) i=lattice%n;
   j=mod(j1,lattice%n); if(j==0) j=lattice%n;

  if(i> lattice%n.or. j>lattice%n+1 ) then
    Write(6,*) " error : magnet does not exist " ,i,j,lattice%n
  endif

   k2=0
  if(j<=i) then
   k1=lattice%n
   k2=j-1
  else 
   k1=j-1
  endif 

   do K=i,k1
    call track(X,LATTICE%MAGNET(K))
   enddo

   do K=1,k2
    call track(X,LATTICE%MAGNET(K))
   enddo

 

END SUBROUTINE track_lattice

SUBROUTINE track_ONE_TURN(X,lattice,I)
IMPLICIT NONE
TYPE(my_taylor), intent(inout) :: X(NVD)
TYPE(my_lattice), intent(in) :: lattice
INTEGER,INTENT(IN):: I
    
	call track(X,LATTICE,I,I)
 

END SUBROUTINE track_ONE_TURN

!&2

!&3
subroutine make_lattice(filename)
implicit none
real(dp) kn(nmult),L
INTEGER K,i,n,mf,KIND,NM,ns
character*(*) filename
character*16 name
N=10
!L1=0.25D0
!K1=20.0D0
!L2=0.2D0
!K2=-k1
!L0=0.1d0
!K0=0.D0
mf=20

open(unit=mf,file=filename,status='old', err=1000)
read(mf,*) n
CALL DESTRUCT(LATTICE)
CALL CONSTRUCT(LATTICE,n)

lattice%n=n



DO k=1,n
 
  
  read(mf,*) name,L,KIND,ns
  IF(KIND/=0) THEN
   READ(mf,*) ( KN(NM), NM=1,NMULT)
   DO NM=1,NMULT
    LATTICE%MAGNET(K)%K(NM)=KN(NM)
   ENDDO
  ENDIF  
  LATTICE%MAGNET(K)%name=name
  LATTICE%MAGNET(K)%n=ns
  LATTICE%MAGNET(K)%L=L;
  LATTICE%MAGNET(K)%type=KIND;
 

ENDDO
close(mf)

 LATTICE%MAGNET(1)%twiss(-1)=0.d0
 LATTICE%MAGNET(1)%twiss(0)= LATTICE%MAGNET(1)%L
DO I=2,lattice%n
 LATTICE%MAGNET(i)%twiss(-1)=LATTICE%MAGNET(i-1)%twiss(0)
 LATTICE%MAGNET(i)%twiss(0)=LATTICE%MAGNET(i-1)%twiss(0)+LATTICE%MAGNET(i)%L
ENDDO

return
1000 continue
ierror=1
message(1)="Lattice file "//filename(1:len_trim(filename))//" not found "
write(6,*) message(1)(1:len_trim(message(1)))

end subroutine make_lattice

!

subroutine write_lattice(filename)
implicit none
real(dp) kn(nmult),L
INTEGER K,i,mf,KIND,NM,ns
character*(*) filename
mf=20

open(unit=mf,file=filename)
write(mf,*) lattice%n




DO k=1,lattice%n
  
  write(mf,'(a16,1x,e20.14,i4,i4)') LATTICE%MAGNET(K)%name, LATTICE%MAGNET(K)%L%r,LATTICE%MAGNET(K)%type,LATTICE%MAGNET(K)%n
  IF(LATTICE%MAGNET(K)%type/=0) THEN
   write(mf,'(4(1x,e20.14))') ( LATTICE%MAGNET(K)%K(NM)%r, NM=1,NMULT)
 endif
ENDDO
close(mf)


end subroutine write_lattice

!

end module my_tracking_code